#include "PathSearch.h"

////////////////////////////////////////////////////
// UNIFORM COST SEARCH
////////////////////////////////////////////////////
namespace fullsail_ai { namespace algorithms {

	PathSearch::PathSearch() : m_OpenList(NodeCompare)
	{
		m_Start = NULL;
		m_Goal = NULL;
		m_TileMap = NULL;
		m_SolutionFound = false;
	}

	PathSearch::~PathSearch()
	{
	}

	bool NodeCompare(PlannerNode* const& first, PlannerNode* const& second)
	{
		return first->givenCost > second->givenCost;
	}

	void PathSearch::areAdjacent(Tile *curTile, Tile *testTile)
	{
		if (curTile == NULL || testTile == NULL || testTile->getWeight() == 0)
			return;

		// Check if were an odd row
		if (curTile->getRow()%2 != 0)
		{
			// Check if test tile is one of the non adjacent tiles within the curTile range
			if ((testTile->getColumn() == (curTile->getColumn()-1) && testTile->getRow() == (curTile->getRow()-1)) ||
				(testTile->getColumn() == (curTile->getColumn()-1) && testTile->getRow() == (curTile->getRow()+1)))
			{
				return;
			}
		}
		// else were even
		else
		{
			// Check if test tile is one of the non adjacent tiles within the curTile range
			if ((testTile->getColumn() == (curTile->getColumn()+1) && testTile->getRow() == (curTile->getRow()-1)) ||
				(testTile->getColumn() == (curTile->getColumn()+1) && testTile->getRow() == (curTile->getRow()+1)))
			{
				return;
			}
		}

		// Finally check to see if we are a valid adjacent tile within the range of curTile
		if (testTile->getColumn() <= (curTile->getColumn()+1) &&
			testTile->getColumn() >= (curTile->getColumn()-1) &&
			testTile->getRow() <= (curTile->getRow()+1) &&
			testTile->getRow() >= (curTile->getRow()-1))
		{
			PlannerNode *newSuccessor = new PlannerNode(testTile);
			m_Successors.push_back(newSuccessor);
		}
	}

	void PathSearch::generateSuccessos(Tile *curTile)
	{
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()-1, curTile->getColumn()-1));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()-1, curTile->getColumn()));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()-1, curTile->getColumn()+1));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow(), curTile->getColumn()-1));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow(), curTile->getColumn()+1));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()+1, curTile->getColumn()-1));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()+1, curTile->getColumn()));
		areAdjacent(curTile, m_TileMap->getTile(curTile->getRow()+1, curTile->getColumn()+1));
	}

	void PathSearch::initialize(TileMap* _tileMap)
	{
		// Store the map we are going to use
		m_TileMap = _tileMap;
	}

	void PathSearch::enter(int startRow, int startColumn, int goalRow, int goalColumn)
	{
		// Create a new PlannerNode at the start tile
		m_Start = m_TileMap->getTile(startRow, startColumn);
		PlannerNode *startNode = new PlannerNode(m_Start);
		// Set goal tile
		m_Goal = m_TileMap->getTile(goalRow, goalColumn);
		// Enqueue the start node into the Open list
		m_OpenList.push(startNode);
		// Associate this newly created node with the start tile in the created map
		m_CreatedList[m_Start] = startNode;
	}

	void PathSearch::update(long timeslice)
	{
		m_Successors.clear();
		while (!m_OpenList.empty())
		{
			// Retrieve the front of the list
			PlannerNode *current = m_OpenList.front();
			// Remove that item from the list 
			m_OpenList.pop();
			if (current->state == m_Goal)
			{
				// Build solution queue
				PlannerNode *tempCur = current;
				while (tempCur != NULL)
				{
					m_SolutionPath.push_back(tempCur->state);
					tempCur = tempCur->parent;
				}
				// Let isDone() know we have found a solution
				m_SolutionFound = true;
				return;
			}
			else
			{
				generateSuccessos(current->state);
				double newGivenCost = 0;
				// For each possible successor (aka only adjacent nodes)
				for (unsigned int succNode = 0; succNode < m_Successors.size(); ++succNode)
				{
					newGivenCost = m_Successors[succNode]->CalcNewGivenCost(current, m_Successors[succNode]);
					// Check to see if it is not a duplicate
					if (NULL != m_CreatedList[m_Successors[succNode]->state])
					{
						// Check if the new given cost is better than the original
						if (m_CreatedList[m_Successors[succNode]->state]->givenCost > newGivenCost)
						{
							// remove the original from the open list
							m_OpenList.remove(m_CreatedList[m_Successors[succNode]->state]);
							// set the original cost = newGivenCost	
							m_Successors[succNode]->givenCost = newGivenCost;
							// set the originals' parent = current
							m_Successors[succNode]->parent = current;
							// push the original onto the open list
							m_OpenList.push(m_Successors[succNode]);
						}
					}
					else
					{
						// Associate the successor's tile with succNode in the created map
						m_CreatedList[m_Successors[succNode]->state] = m_Successors[succNode];
						// Set the given cost of the successor node
						m_Successors[succNode]->givenCost = newGivenCost;
						// Set the parent
						m_CreatedList[m_Successors[succNode]->state]->parent = current;
						// Push back successor onto open list
						m_OpenList.push(m_Successors[succNode]);
					}
				}
			}
			if (timeslice == 0)
				break;
		}

		// Draw Code
// 		unsigned int size = m_OpenList.size();
// 		for (unsigned int i = 0; i < size; ++i)
// 		{
// 			unsigned int weight = 0;
// 			weight = m_OpenList.open[i]->state->getWeight();
// 			if (weight == 1)
// 				m_OpenList.open[i]->state->setFill(0xBB0000FF);
// 			else if (weight == 9)
// 				m_OpenList.open[i]->state->setFill(0xFFFF0000);
// 			else
// 				m_OpenList.open[i]->state->setFill(0xFFFFFF00);
// 		}
	}

	void PathSearch::shutdown()
	{
		// Clean up the memory tile map contains
		if (m_TileMap)
			m_TileMap->reset();
		m_SolutionPath.clear();
		m_OpenList.clear();
		m_CreatedList.clear();
	}

	void PathSearch::exit()
	{
		
	}

	bool PathSearch::isDone() const
	{
		return m_SolutionFound;
	}

	std::vector<Tile const*> const PathSearch::getSolution() const
	{
		//std::vector<Tile const*> temp;
		return m_SolutionPath;
	}
}}  // namespace fullsail_ai::algorithms

